#include "imu_task.h"

float ist8310_real_data[3] = {0};
float INS_quat_realtime[4] = {0.0f, 0.0f, 0.0f, 0.0f};
float local_vec[3] = {0.0f, 0.0f, 0.0f};

uint8_t imu_read = 0;

void imu_init_reg_func(){
	REGISTER_IMU_INIT_PACK(&imu2_init_list, DEVICE_CONFIG_REG, 0xFF, SET_REG, DEVICE_CONFIG_SOFT_RESET, 100);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, REG_BANK_SEL_REG, 0xFF, SET_REG, REG_BANK_SEL_0, 0);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, FIFO_CONFIG_REG, 0xFF, SET_REG, FIFO_CONFIG_FIFO_MODE_STREAM, 10);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, INT_SOURCE0_REG, 0xFF, GET_REG, 0xFF, 0);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, INT_SOURCE0_REG, INT_SOURCE0_REG, SET_FROM_REG, 0xFF, 10);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, FIFO_CONFIG2_REG, 0xFF, SET_REG, (EACH_FRAME_LENGTH * FRAME_MAX_CNT) & 0xFF, 10);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, FIFO_CONFIG3_REG, 0xFF, SET_REG, (EACH_FRAME_LENGTH * FRAME_MAX_CNT >> 8) & 0x07, 10);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, INT_SOURCE0_REG, INT_SOURCE0_REG, SET_FROM_REG, 0xFF, 10);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, FIFO_CONFIG1_REG, 0xFF, SET_REG, FIFO_CONFIG1_ACCEL_EN |
				FIFO_CONFIG1_GYRO_EN |
				FIFO_CONFIG1_RESUME_PARTIAL_RD_TRUE |
				FIFO_CONFIG1_WM_GT_TH, 10);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, INT_CONFIG_REG, 0xFF, SET_REG, INT_CONFIG_INT1_MODE_LATCHED_MODE |INT_CONFIG_INT1_DRIVE_CIRCUIT_PUSH_PULL | INT_CONFIG_INT1_POLARITY_ACTIVE_HIGH, 10);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, INT_SOURCE0_REG, 0xFF, GET_REG, 0xFF, 0);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, INT_SOURCE0_REG, INT_SOURCE0_REG, SET_FROM_REG, INT_SOURCE0_FIFO_THS_INT1_EN_ROUTED | INT_SOURCE0_FIFO_FULL_INT1_EN_ROUTED, 10);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, GYRO_CONFIG0_REG, 0xFF, SET_REG, imu2.gyro_max | GYRO_CONFIG0_ODR_1K, 10);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, ACCEL_CONFIG0_REG, 0xFF, SET_REG, imu2.accel_max | ACCEL_CONFIG0_ODR_1K, 10);
	REGISTER_IMU_INIT_PACK(&imu2_init_list, PWR_MGMT0_REG, 0xFF, SET_REG, PWR_MGMT0_TEMP_DISABLE | PWR_MGMT0_GYRO_MODE_LN | PWR_MGMT0_ACCEL_MODE_LN, 200);
	imu2.gyro_dps = 0.06103515625f * 0.01745329252f;
	imu2.accel_dps = 0.00478515625f;
	
	REGISTER_IMU_INIT_PACK(&imu1_init_list, DEVICE_CONFIG_REG, 0xFF, SET_REG, DEVICE_CONFIG_SOFT_RESET, 100);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, REG_BANK_SEL_REG, 0xFF, SET_REG, REG_BANK_SEL_0, 0);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, FIFO_CONFIG_REG, 0xFF, SET_REG, FIFO_CONFIG_FIFO_MODE_STREAM, 10);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, INT_SOURCE0_REG, 0xFF, GET_REG, 0xFF, 0);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, INT_SOURCE0_REG, INT_SOURCE0_REG, SET_FROM_REG, 0xFF, 10);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, FIFO_CONFIG2_REG, 0xFF, SET_REG, (EACH_FRAME_LENGTH * FRAME_MAX_CNT) & 0xFF, 10);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, FIFO_CONFIG3_REG, 0xFF, SET_REG, (EACH_FRAME_LENGTH * FRAME_MAX_CNT >> 8) & 0x07, 10);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, INT_SOURCE0_REG, INT_SOURCE0_REG, SET_FROM_REG, 0xFF, 10);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, FIFO_CONFIG1_REG, 0xFF, SET_REG, FIFO_CONFIG1_ACCEL_EN |
				FIFO_CONFIG1_GYRO_EN |
				FIFO_CONFIG1_RESUME_PARTIAL_RD_TRUE |
				FIFO_CONFIG1_WM_GT_TH, 10);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, INT_CONFIG_REG, 0xFF, SET_REG, INT_CONFIG_INT1_MODE_LATCHED_MODE |INT_CONFIG_INT1_DRIVE_CIRCUIT_PUSH_PULL | INT_CONFIG_INT1_POLARITY_ACTIVE_HIGH, 10);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, INT_SOURCE0_REG, 0xFF, GET_REG, 0xFF, 0);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, INT_SOURCE0_REG, INT_SOURCE0_REG, SET_FROM_REG, INT_SOURCE0_FIFO_THS_INT1_EN_ROUTED | INT_SOURCE0_FIFO_FULL_INT1_EN_ROUTED, 10);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, GYRO_CONFIG0_REG, 0xFF, SET_REG, imu2.gyro_max | GYRO_CONFIG0_ODR_1K, 10);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, ACCEL_CONFIG0_REG, 0xFF, SET_REG, imu2.accel_max | ACCEL_CONFIG0_ODR_1K, 10);
	REGISTER_IMU_INIT_PACK(&imu1_init_list, PWR_MGMT0_REG, 0xFF, SET_REG, PWR_MGMT0_TEMP_DISABLE | PWR_MGMT0_GYRO_MODE_LN | PWR_MGMT0_ACCEL_MODE_LN, 200);
	imu1.gyro_dps = 0.06103515625f * 0.01745329252f;
	imu1.accel_dps = 0.00478515625f;
}

void get_mixer_imu_data(void){
	dart_attitude.accels[0] = 0;
	dart_attitude.accels[1] = 0;
	dart_attitude.accels[2] = 0;
	if(imu1.imu_online_if){
		dart_attitude.accels[0] += imu1.imu_original_data.ax;
		dart_attitude.accels[1] += imu1.imu_original_data.ay;
		dart_attitude.accels[2] += imu1.imu_original_data.az;
		if(dart_attitude.imu_state[0] == false){
			dart_attitude.imu_state[0] = true;
			dart_attitude.imu_num++;
		}
	}
	if(imu2.imu_online_if){
		dart_attitude.accels[0] += imu2.imu_original_data.ax;
		dart_attitude.accels[1] += imu2.imu_original_data.ay;
		dart_attitude.accels[2] += imu2.imu_original_data.az;
		if(dart_attitude.imu_state[1] == false){
			dart_attitude.imu_state[1] = true;
			dart_attitude.imu_num++;
		}
	}
	if(dart_attitude.imu_num > 0){
		dart_attitude.accels[0] /= (float)dart_attitude.imu_num;
		dart_attitude.accels[1] /= (float)dart_attitude.imu_num;
		dart_attitude.accels[2] /= (float)dart_attitude.imu_num;
	}
	
	dart_attitude.gyros[0] = 0;
	dart_attitude.gyros[1] = 0;
	dart_attitude.gyros[2] = 0;
	if(imu1.imu_online_if){
		dart_attitude.gyros[0] += imu1.imu_original_data.gx;
		dart_attitude.gyros[1] += imu1.imu_original_data.gy;
		dart_attitude.gyros[2] += imu1.imu_original_data.gz;
	}
	if(imu2.imu_online_if){
		dart_attitude.gyros[0] += imu2.imu_original_data.gx;
		dart_attitude.gyros[1] += imu2.imu_original_data.gy;
		dart_attitude.gyros[2] += imu2.imu_original_data.gz;
	}
	if(dart_attitude.imu_num > 0){
		dart_attitude.gyros[0] /= (float)dart_attitude.imu_num;
		dart_attitude.gyros[1] /= (float)dart_attitude.imu_num;
		dart_attitude.gyros[2] /= (float)dart_attitude.imu_num;
	}
}

void calc_angle(){
	get_mixer_imu_data();
	if(not dart_attitude.ahrs_init){
		AHRS_init(INS_quat_realtime, dart_attitude.accels, ist8310_real_data);
		dart_attitude.ahrs_init = true;
	}
	if(dart_attitude.ahrs_init){
		AHRS_update(INS_quat_realtime, 0.001f, dart_attitude.gyros, dart_attitude.accels, ist8310_real_data);
		get_angle(INS_quat_realtime, &dart_attitude.imu_angle.yaw, &dart_attitude.imu_angle.pitch, &dart_attitude.imu_angle.roll);
		dart_attitude.imu_angle.pitch *= RAD_TO_ANGLE;
		dart_attitude.imu_angle.roll *= RAD_TO_ANGLE;
		dart_attitude.imu_angle.yaw *= RAD_TO_ANGLE;
	}
}

void imu_task(void const * argument){
	imu_init_reg_func();
	while(1){
		imu_auto(&imu2);
		imu_auto(&imu1);
		calc_angle();
		osDelay(1);
	}
}

uint16_t watch_gpio;

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin){
	watch_gpio = GPIO_Pin;
	if(GPIO_Pin == GYRO_INT2_Pin){
			if(imu2.imu_online_if){
				uint8_t dummy;
				get_icm_reg(&imu2, 0x2D, &dummy, 1);
				imu2.fifo_ready = true;
			}
	}
	if(GPIO_Pin == GYRO_INT1_Pin){
		if(imu1.imu_online_if){
			uint8_t dummy;
			get_icm_reg(&imu1, 0x2D, &dummy, 1);
			imu1.fifo_ready = true;
		}
	}
}
